﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;
using System.Globalization;

namespace DTW_Beta
{
    internal class DtwGestureRecognizer
    {
        // Size of obeservations vectors.
        private readonly int _dimension;
       
        // Maximum distance between the last observations of each sequence.
        private readonly double _firstThreshold;

        // Minimum length of a gesture before it can be recognised.
        private readonly int _minimumLength;

        // Maximum DTW distance between an example and a sequence being classified.
        private readonly double _globalThreshold;

        // The gesture names. Index matches that of the sequences array in _sequences
        private readonly ArrayList _labels;

        // Maximum vertical or horizontal steps in a row.
        private readonly int _maxSlope;

        // The recorded gesture sequences
        private readonly ArrayList _sequences;

        // Weights used for weighting gesture recognition to have a more roboust system.
        private readonly ArrayList _weightsList;

        // Main windows to update text on the objects on the window
        private MainWindow _window;
       
        // First constructor.
        public DtwGestureRecognizer(MainWindow window, int dim, double threshold, double firstThreshold, int minLen)
        {
            _window = window;
            _dimension = dim;
            _sequences = new ArrayList();
            _labels = new ArrayList();
            _weightsList = new ArrayList();
            _globalThreshold = threshold;
            _firstThreshold = firstThreshold;
            _maxSlope = int.MaxValue;
            _minimumLength = minLen;
        }
        
        // Second constructor.
        public DtwGestureRecognizer(MainWindow window, int dim, double threshold, double firstThreshold, int maxSlope, int  minLen)
            :this(window, dim, threshold, firstThreshold, minLen)
        {
            _maxSlope = maxSlope;
        }

        // Adds or updates a new sequence.
        public void AddOrUpdate(ArrayList sequence, string label, double[] weights)
        {
            // First we check whether there is already a recording for this label. If so overwrite it, otherwise add a new entry
            int existingIndex = -1;

            for (int i = 0; i < _labels.Count; i++)
            {
                if ((string)_labels[i] == label)
                {
                    existingIndex = i;
                }
            }

            // If we have a match then remove the entries at the existing index to avoid duplicates. We will add the new entries later anyway
            if (existingIndex >= 0)
            {
                _sequences.RemoveAt(existingIndex);
                _labels.RemoveAt(existingIndex);
            }

            // Add the new entries
            _sequences.Add(sequence);
            _weightsList.Add(weights);
            _labels.Add(label);
        }

        public double[] GetJointCenters(ArrayList sequence)
        {
            double[] rVal = new double[12];

            //Calculate total displacements.
            for (int i = 0; i < sequence.Count; i++)
            {
                double[] seqArray= (double[])sequence[i];

                for (int j = 0; j < 12; j++)
                {
                    rVal[j] += seqArray[j];
                }
            }

            for (int i = 0; i < 12; i++)
            {
                rVal[i] = rVal[i] / sequence.Count;
            }

            return rVal;
        }

        public void AdaptiveCentering(ref ArrayList sequence)
        {
            double[] weights = GetWeights(sequence);
            double[] jointCenters = GetJointCenters(sequence);

            double gestureCenterX = 0;
            double gestureCenterY = 0;

            gestureCenterX = jointCenters[10];
            gestureCenterY = jointCenters[11];

            for (int i = 0; i < sequence.Count; i++)
            {
                double[] example = (double[])sequence[i];

                for (int j = 0; j < 6; j++)
                {
                    example[2 * j] = example[2 * j] - gestureCenterX;
                    example[2 * j + 1] = example[2 * j + 1] - gestureCenterY;
                }
            }
        }

        // Recognize gesture in the given sequence.
        public string Recognize(ArrayList sequence)
        {

            //AdaptiveCentering(ref sequence);

            double minDist = double.PositiveInfinity;
            string classification = "__UNKNOWN";
            int classificationIndex = 0;
            for (int i = 0; i < _sequences.Count; i++)
            {
                var example = (ArrayList)_sequences[i];
                
                double temp = Distance((double[])sequence[sequence.Count - 1], (double[])example[example.Count - 1], (double[])_weightsList[i]);

                if ( temp < _firstThreshold)
                {
                    double d = Dtw(sequence, example, (double[])_weightsList[i]) / example.Count;
                    if (d < minDist)
                    {
                        minDist = d;
                        classification = (string)_labels[i];
                        classificationIndex = i;
                    }
                }
            }

            if (minDist < _globalThreshold)
            {
                return classification;
            }

            return "__UNKNOWN";
        }
        
        // Compute the min DTW distance between seq2 and all possible endings of seq1.
        public double Dtw(ArrayList seq1, ArrayList seq2, double[] weights)
        {
            // Init
            var seq1R = new ArrayList(seq1);
            seq1R.Reverse();
            var seq2R = new ArrayList(seq2);
            seq2R.Reverse();

            var tab = new double[seq1R.Count + 1, seq2R.Count + 1];
            var slopeI = new int[seq1R.Count + 1, seq2R.Count + 1];
            var slopeJ = new int[seq1R.Count + 1, seq2R.Count + 1];

            for (int i = 0; i < seq1R.Count + 1; i++)
            {
                for (int j = 0; j < seq2R.Count + 1; j++)
                {
                    tab[i, j] = double.PositiveInfinity;
                    slopeI[i, j] = 0;
                    slopeJ[i, j] = 0;
                }
            }

            tab[0, 0] = 0;

            // Dynamic computation of the DTW matrix.
            for (int i = 1; i < seq1R.Count + 1; i++)
            {
                for (int j = 1; j < seq2R.Count + 1; j++)
                {
                    if (tab[i, j - 1] < tab[i - 1, j - 1] && tab[i, j - 1] < tab[i - 1, j] &&
                        slopeI[i, j - 1] < _maxSlope)
                    {
                        tab[i, j] = Distance((double[])seq1R[i - 1], (double[])seq2R[j - 1], (double[])weights) + tab[i, j - 1];
                        slopeI[i, j] = slopeI[i, j - 1] + 1;
                        slopeJ[i, j] = 0;
                    }
                    else if (tab[i - 1, j] < tab[i - 1, j - 1] && tab[i - 1, j] < tab[i, j - 1] &&
                             slopeJ[i - 1, j] < _maxSlope)
                    {
                        tab[i, j] = Distance((double[])seq1R[i - 1], (double[])seq2R[j - 1], (double[])weights) + tab[i - 1, j];
                        slopeI[i, j] = 0;
                        slopeJ[i, j] = slopeJ[i - 1, j] + 1;
                    }
                    else
                    {
                        tab[i, j] = Distance((double[])seq1R[i - 1], (double[])seq2R[j - 1], (double[])weights) + tab[i - 1, j - 1];
                        slopeI[i, j] = 0;
                        slopeJ[i, j] = 0;
                    }
                }
            }

            // Find best between seq2 and an ending (postfix) of seq1.
            double bestMatch = double.PositiveInfinity;
            for (int i = 1; i < seq1R.Count + 1; i++)
            {
                if (tab[i, seq2R.Count] < bestMatch)
                {
                    bestMatch = tab[ i, seq2R.Count ];
                }
            }

            return bestMatch;
        }

        // Computes a 2-distance between two observations. (aka Euclidian distance).
        private double Distance(double[] a, double[] b, double[] weights)
        {
            double d = 0;
            for (int i = 0; i < _dimension / 2; i++)
            {
                double euclideanDist = Math.Sqrt(Math.Pow(a[2 * i] - b[2 * i], 2) + Math.Pow(a[2 * i + 1] - b[2 * i + 1], 2));
                d += euclideanDist * weights[i];
            }

            return d;
        }

        public double[] GetWeights(ArrayList _video)
        {
            double[] totalDisplacements = new double[6];
            double[] a = new double[6];
            double[] weights = new double[6];
            double weightSum = 0;

            double T1 = 0.035;
            double T2 = 0.25;
            double BETA = 2;

            //Calculate total displacements.
            for (int i = 0; i < _video.Count - 1; i++)
            {
                double[] leftSeq  = (double[])_video[i];
                double[] rightSeq = (double[])_video[i + 1];
                for (int j = 0; j < 6; j++)
                {
                    totalDisplacements[j] += Math.Sqrt(Math.Pow(leftSeq[2 * j + 1] - rightSeq[2 * j + 1], 2) + Math.Pow(leftSeq[2 * j] - rightSeq[2 * j], 2));
                }
            }

            // Use f(x) function to totalDisplacements.
            for (int i = 0; i < 6; i++)
            {
                if (totalDisplacements[i] < T1)
                {
                    a[i] = 0;
                }
                else if (totalDisplacements[i] < T2)
                {
                    a[i] = (totalDisplacements[i] - T1) / (T2 - T1);
                }
                else
                {
                    a[i] = 1;
                }
                weights[i] = 1 - Math.Exp(-1 * BETA * a[i]);
                weightSum += weights[i];
            }

            //Calculate total weights.
            for (int i = 0; i < 6; i++)
            {
                weights[i] = weights[i] / (weightSum);
            }

            return weights;
        }

        // Didn't test. Test it!
        public string RetrieveText()
        {
            string retStr = String.Empty;

            if (_sequences != null)
            {
                // Iterate through each gesture
                for (int gestureNum = 0; gestureNum < _sequences.Count; gestureNum++)
                {
                    // Echo the label
                    retStr += _labels[gestureNum] + "\r\n";

                    int frameNum = 0;

                    //Iterate through each frame of this gesture
                    foreach (double[] frame in ((ArrayList)_sequences[gestureNum]))
                    {
                        // Extract each double
                        foreach (double dub in (double[])frame)
                        {
                            retStr += dub.ToString(CultureInfo.InvariantCulture) + "\r\n";
                        }

                        // Signifies end of this double
                        retStr += "~\r\n";

                        frameNum++;
                    }

                    retStr += "*\r\n";
                    for (int i = 0; i < 6; i++)
                    {
                        retStr += ((double[])_weightsList[gestureNum])[i].ToString(CultureInfo.InvariantCulture) + "\r\n";
                    }

                    // Signifies end of this gesture
                    retStr += "----";
                    if (gestureNum < _sequences.Count - 1)
                    {
                        retStr += "\r\n";
                    }
                }
            }

            return retStr;
        }
    }
}
